
/*
 * @Author: ilikara 3435193369@qq.com
 * @Date: 2025-01-04 06:50:56
 * @LastEditors: ilikara 3435193369@qq.com
 * @LastEditTime: 2025-04-12 09:49:22
 * @FilePath: /2k300_smartcar/src/image_cv.cpp
 * @Description: 智能车图像处理模块，包含颜色检测、道路识别、路径跟踪等功能
 */
#include "image_cv.h"
#include <iostream>
// #include <iostream>
// #include <sys/ioctl.h>
// #include <termios.h>
// #include <unistd.h>
// #include <unordered_set>

// #include "GPIO.h"
// #include "MotorController.h"
// #include "PwmController.h"

/*使能gpio引脚73
*/
GPIO mortorEN(73);
MotorController* motorController[2] = { nullptr, nullptr };
PwmController servo(1, 0);


// 全局图像变量，存储不同处理阶段的图像数据
cv::Mat raw_frame;          // 原始图像帧
cv::Mat grayFrame;          // 灰度图像
cv::Mat binarizedFrame;     // 二值化图像
cv::Mat morphologyExFrame;  // 形态学处理后图像
cv::Mat track;              // 道路跟踪掩码

// 全局线条变量，存储各行列号位置（-1表示未检测到）
std::vector<int> left_line;  // 左边缘列号数组
std::vector<int> right_line; // 右边缘列号数组
std::vector<int> mid_line;   // 中线列号数组

// 图像尺寸变量
int line_tracking_width;  // 跟踪区域宽度
int line_tracking_height; // 跟踪区域高度

// 黄色区域计数变量
int yellowAreaCount;


//全局控制参数定义
const int CENTER_X = line_tracking_width / 2;    // 图像中心列坐标
const int BASE_DUTY = 1500000;                  // 舵机中位占空比
const int DUTY_STEP = 50000;                    // 每像素偏移量对应的占空比变化量
const int MAX_DUTY = 1680000;                   // 最大右转占空比
const int MIN_DUTY = 1360000;                   // 最大左转占空比
const int BASE_SPEED = 1500000;                 // 电机基准速度（占空比）


/**
 * @brief 检测图像中的黄色块数量
 * @param inputImage 输入的BGR图像
 * @return 黄色块数量（检测失败返回-1）
 * @note 使用HSV颜色空间过滤黄色区域，通过轮廓检测和面积过滤识别有效块
 */
int detectYellowBlocks(const cv::Mat& inputImage)
{
    // 检查输入图像有效性
    if (inputImage.empty()) {
        std::cerr << "Error: Input image is empty!" << std::endl;
        return -1;
    }

    // 转换至HSV颜色空间（更适合颜色分割）
    cv::Mat hsvImage;
    cvtColor(inputImage, hsvImage, cv::COLOR_BGR2HSV);

    // 定义黄色HSV范围（需根据实际环境调整）
    cv::Scalar lowerYellow(10, 100, 100);  // 低阈值：H(10-40)接近黄色区间
    cv::Scalar upperYellow(40, 255, 255);   // 高阈值：覆盖较宽黄色范围

    // 创建黄色区域掩膜
    cv::Mat yellowMask;
    inRange(hsvImage, lowerYellow, upperYellow, yellowMask);

    // 形态学去噪：开运算（先腐蚀后膨胀）去除小噪声点
    cv::Mat kernel = getStructuringElement(cv::MORPH_RECT, cv::Size(2, 2));
    morphologyEx(yellowMask, yellowMask, cv::MORPH_OPEN, kernel);
    morphologyEx(yellowMask, yellowMask, cv::MORPH_CLOSE, kernel); // 闭运算连接邻近区域

    // 查找外部轮廓
    std::vector<std::vector<cv::Point>> contours;
    std::vector<cv::Vec4i> hierarchy;
    findContours(yellowMask, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);

    // 过滤小轮廓（面积阈值控制）
    int minContourArea = 4;       // 最小有效轮廓面积
    int yellowBlockCount = 0;     // 有效黄色块计数

    for (size_t i = 0; i < contours.size(); i++) {
        double area = contourArea(contours[i]); // 计算轮廓面积
        if (area > minContourArea) {             // 过滤小面积噪声
            yellowBlockCount++;
        }
    }

    return yellowBlockCount;
}

/**
 * @brief 图像二值化处理（基于HSV通道阈值）
 * @param frame 输入的BGR图像
 * @return 二值化后的单通道图像
 * @note 对H通道和S通道分别应用大津阈值，合并结果突出道路区域
 */
static inline cv::Mat image_binerize(cv::Mat& frame)
{
    cv::Mat output;               // 输出二值化图像
    cv::Mat binarizedFrame;       // 临时存储H通道二值化结果
    cv::Mat hsvImage;             // HSV颜色空间图像

    cvtColor(frame, hsvImage, cv::COLOR_BGR2HSV); // 转换至HSV

    std::vector<cv::Mat> hsvChannels;
    split(hsvImage, hsvChannels); // 分离HSV通道（H=0, S=1, V=2）

    // 对H通道应用大津阈值（反二值化：背景为0，目标为255）
    threshold(hsvChannels[0], binarizedFrame, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
    // 对S通道应用大津阈值（反二值化）
    threshold(hsvChannels[1], output, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
    // 合并两个通道的二值化结果
    bitwise_or(output, binarizedFrame, output);

    return output;
}

/**
 * @brief 道路区域检测（基于洪水填充算法）
 * @param frame 输入的二值化图像（预处理后的道路掩码）
 * @return 标记道路区域的掩码图像
 * @note 以图像底部中心为种子点，通过洪水填充分割道路区域
 */
static inline cv::Mat find_road(cv::Mat& frame)
{
    // 形态学开运算：去除小噪声，保留道路结构
    cv::Mat kernel = getStructuringElement(cv::MORPH_CROSS, cv::Size(2, 2)); // 十字形核
    morphologyEx(binarizedFrame, morphologyExFrame, cv::MORPH_OPEN, kernel);

    // 创建洪水填充掩码（需比图像大2像素边界）
    cv::Mat mask = Mat::zeros(line_tracking_height + 2, line_tracking_width + 2, CV_8UC1);

    // 种子点设置：图像底部中心偏上10像素（假设道路在图像下方）
    cv::Point seedPoint(line_tracking_width / 2, line_tracking_height - 10);

    // 在预处理图像中标记种子点区域（用于填充起始）
    circle(morphologyExFrame, seedPoint, 10, 255, -1); // 画实心圆，半径10像素

    // 洪水填充参数
    cv::Scalar newVal(128);        // 填充值（灰度128表示道路区域）
    cv::Scalar loDiff = Scalar(20); // 低差异阈值
    cv::Scalar upDiff = Scalar(20); // 高差异阈值

    // 执行洪水填充（8邻域，带掩码限制）
    floodFill(morphologyExFrame, mask, seedPoint, newVal, 0, loDiff, upDiff, 8);

    // 提取有效区域：去除掩码边界，生成最终道路掩码
    cv::Mat outputImage = Mat::zeros(line_tracking_width, line_tracking_height, CV_8UC1);
    mask(Rect(1, 1, line_tracking_width, line_tracking_height)).copyTo(outputImage); // 掩码偏移1像素去除边界

    return outputImage;
}


/* @void Init:demo1键盘控制，使能引脚部分
*/
void Init()
{
    mortorEN.setDirection("out");
    mortorEN.setValue(1);
    const int pwmchip[2] = { 8, 8 };
    const int pwmnum[2] = { 2, 1 };
    const int gpioNum[2] = { 12, 13 };
    const int encoder_pwmchip[2] = { 0, 3 };
    const int encoder_gpioNum[2] = { 75, 72 };
    const int encoder_dir[2] = { 1, -1 };
    const unsigned int period_ns = 50000; // 20 kHz

    for (int i = 0; i < 2; ++i) {
        motorController[i] = new MotorController(pwmchip[i], pwmnum[i], gpioNum[i], period_ns,
            0, 0, 0, 0,
            encoder_pwmchip[i], encoder_gpioNum[i], encoder_dir[i]);
    }

    servo.setPeriod(3000000);
    servo.setDutyCycle(1500000);
    servo.enable();
}


/**
 * @brief 主图像处理流程（包含图像预处理、线条检测、中线计算）
 * @note 处理流程：图像缩放 -> 黄色检测 -> 二值化 -> 道路分割 -> 线条跟踪 -> 中线生成
 */
void image_main()
{
    cv::Mat resizedFrame; // 存储缩放后的图像

    // 调整原始帧尺寸至跟踪区域大小
    resize(raw_frame, resizedFrame, Size(line_tracking_width, line_tracking_height));

    // 检测黄色区域数量（用于判断是否进入弯道处理）
    yellowAreaCount = detectYellowBlocks(resizedFrame);
    // std::cout << " Yellow Area:" << yellowAreaCount << std::endl; // 调试输出

    // 图像二值化处理
    binarizedFrame = image_binerize(resizedFrame);

    // 道路区域检测（获取道路掩码）
    track = find_road(binarizedFrame);

    // 初始化线条数组（重置为-1表示未检测到）
    left_line.clear();
    right_line.clear();
    mid_line.clear();
    left_line.resize(line_tracking_height, -1);
    right_line.resize(line_tracking_height, -1);
    mid_line.resize(line_tracking_height, -1);

    // 获取图像数据指针（行优先访问）
    uchar(*IMG)[line_tracking_width] = reinterpret_cast<uchar(*)[line_tracking_width]>(track.data);

    // 逐行扫描检测左右边界（从下往上，跳过底部10行）
    for (int i = line_tracking_height - 1; i >= 10; --i) {
        int max_start = -1;       // 最大连续区域起始列
        int max_end = -1;         // 最大连续区域结束列
        int current_start = -1;   // 当前连续区域起始列
        int current_length = 0;   // 当前连续区域长度
        int max_length = 0;       // 最大连续区域长度

        // 逐列扫描当前行
        for (int j = 0; j < line_tracking_width; ++j) {
            if (IMG[i][j]) { // 当前像素为道路区域（非0值）
                if (current_length == 0) { // 新连续区域开始
                    current_start = j;
                    current_length = 1;
                } else { // 扩展当前连续区域
                    current_length++;
                }
                // 处理区域结束情况（行末或遇到非道路像素）
                if (j == line_tracking_width - 1 || IMG[i][j + 1] == 0) {
                    // 条件：若当前行的下一行边界未超出当前区域范围，则更新最大区域
                    if (i == line_tracking_height - 1 || !(left_line[i + 1] > j || right_line[i + 1] < current_start)) {
                        if (current_length >= max_length) { // 保留最长连续区域
                            max_length = current_length;
                            max_start = current_start;
                            max_end = j;
                        }
                    }
                }
            } else { // 非道路像素，重置当前区域
                current_length = 0;
                current_start = -1;
            }
        }
        // 更新当前行左右边界（若有有效区域）
        if (max_length > 0) {
            left_line[i] = max_start;
            right_line[i] = max_end;
        } else { // 无有效区域，标记为-1
            left_line[i] = -1;
            right_line[i] = -1;
        }
    }

    /*
    // 被注释的线条拟合逻辑（用于处理边界跳变）
    bool left_mark = false, right_mark = false;
    // 从下往上扫描检测边界跳变（处理底部1/4区域）
    for (int row = line_tracking_height - 10; row >= line_tracking_height * 3 / 4; --row) {
        if (left_mark && right_mark) break; // 左右标记均找到则退出

        // 检测左边界跳变（当前行与下一行左边界差值超过10像素）
        if (!left_mark && left_line[row] - left_line[row - 1] > 10) {
            left_mark = row; // 记录跳变行
            // 用跳变点下方5行数据拟合左边界直线（最小二乘法）
            float left_sum_x = 0, left_sum_y = 0, left_sum_xy = 0, left_sum_xx = 0;
            int start_row = row + 1; // 确保不越界（取跳变行的下一行）
            int end_row = row + 5;   // 最多取5行数据
            int n = end_row - start_row + 1;

            for (int i = start_row; i <= end_row; ++i) {
                left_sum_x += i;       // 行号累加（x坐标）
                left_sum_y += left_line[i]; // 列号累加（y坐标）
                left_sum_xy += i * left_line[i]; // 乘积累加
                left_sum_xx += i * i; // 行号平方累加
            }

            // 计算直线参数y = a*x + b（最小二乘解）
            float left_a = (n * left_sum_xy - left_sum_x * left_sum_y) / (n * left_sum_xx - left_sum_x * left_sum_x);
            float left_b = (left_sum_y - left_a * left_sum_x) / n;

            // 用拟合直线预测跳变行上方的左边界值
            for (int i = row - 1; i >= line_tracking_height * 3 / 4; --i) {
                left_line[i] = static_cast<int>(left_a * i + left_b);
            }
        }

        // 检测右边界跳变（逻辑与左边界对称）
        if (!right_mark && right_line[row] - right_line[row - 1] > 10) {
            right_mark = row;
            float right_sum_x = 0, right_sum_y = 0, right_sum_xy = 0, right_sum_xx = 0;
            int start_row = row + 1;
            int end_row = row + 5;
            int n = end_row - start_row + 1;

            for (int i = start_row; i <= end_row; ++i) {
                right_sum_x += i;
                right_sum_y += right_line[i];
                right_sum_xy += i * right_line[i];
                right_sum_xx += i * i;
            }

            float right_a = (n * right_sum_xy - right_sum_x * right_sum_y) / (n * right_sum_xx - right_sum_x * right_sum_x);
            float right_b = (right_sum_y - right_a * right_sum_x) / n;

            for (int i = row - 1; i >= line_tracking_height * 3 / 4; --i) {
                right_line[i] = static_cast<int>(right_a * i + right_b);
            }
        }
    }
    */

    // 计算中线：从下往上，若当前行左右边界有效则取中点，否则继承下一行中线
    for (int row = line_tracking_height - 1; row >= 0; --row) {
        if (left_line[row] != -1) { // 左右边界均有效时计算中点
            mid_line[row] = (left_line[row] + right_line[row]) / 2;
        } else { // 无效时沿用下一行中线（假设中线连续）
            mid_line[row] = mid_line[row + 1];
        }
    }

    // 黄色区域较多时（可能为弯道），进行中线校正
    if (yellowAreaCount >= 7) {
        int left_mark = 0, right_mark = 0; // 记录左右边界跳变行
        // 从下往上扫描检测边界跳变（全高度范围）
        for (int row = line_tracking_height - 10; row >= 0; --row) {
            if (left_line[row] == -1 || right_line[row] == -1) break; // 遇到无效边界则终止

            // 检测左边界上跳变（当前行左边界小于下一行左边界，差值>5）
            if (!left_mark && left_line[row] - left_line[row + 1] > 5) {
                left_mark = row;
            }
            // 检测右边界下跳变（下一行右边界大于当前行右边界，差值>5）
            if (!right_mark && right_line[row + 1] - right_line[row] > 5) {
                right_mark = row;
            }

            // 左右跳变均检测到时，计算中线斜率并插值
            if (left_mark && right_mark) {
                // 计算跳变区域中心点坐标
                double out_x = (left_line[left_mark] + right_line[right_mark]) / 2.0;
                double out_y = (left_mark + right_mark) / 2.0;
                // 计算中线斜率（相对于图像底部中心）
                double k = (out_x - line_tracking_width / 2.0) / (out_y - (line_tracking_height - 1));
                // 用斜率预测跳变区域上方的中线值
                for (int i = line_tracking_height - 1; i >= MIN(left_mark, right_mark) - 5; --i) {
                    mid_line[i] = (i - (line_tracking_height - 1)) * k + (line_tracking_width / 2.0);
                }
                break; // 处理一次后退出
            }
        }
    }
}


/**
 * @brief 根据中线偏移量调整舵机和电机
 * @param mid_col 底部中线列坐标
 */
void controlCar(int mid_col) {
    static int last_duty = BASE_DUTY; // 上次舵机占空比
    
    // 计算中线偏移量（图像中心为基准）
    int error = mid_col - CENTER_X;
    
    // 计算目标占空比（比例控制）
    int target_duty = BASE_DUTY + error * DUTY_STEP;
    
    // 限幅保护
    target_duty = std::clamp(target_duty, MIN_DUTY, MAX_DUTY);
    
    // 更新舵机占空比（仅当变化超过阈值时调整）
    if(abs(target_duty - last_duty) > 1000) {
        servo.setDutyCycle(target_duty);
        last_duty = target_duty;
    }

    // 电机差速控制（根据转向需求）
    if(error > 5) { // 中线偏右，左转：右轮加速，左轮减速
        motorController[0]->setSpeed(BASE_SPEED * 0.8); // 左轮
        motorController[1]->setSpeed(BASE_SPEED * 1.2); // 右轮
    } else if(error < -5) { // 中线偏左，右转：左轮加速，右轮减速
        motorController[0]->setSpeed(BASE_SPEED * 1.2); // 左轮
        motorController[1]->setSpeed(BASE_SPEED * 0.8); // 右轮
    } else { // 直行状态
        motorController[0]->setSpeed(BASE_SPEED);
        motorController[1]->setSpeed(BASE_SPEED);
    }
}



int mian()
{
    Init();
    while(true) {
            // 获取原始图像帧
            cap >> raw_frame;
            if(raw_frame.empty()) break;
        
            // 执行图像处理流程（包含中线计算）
            image_main();
        
            // 获取底部中线坐标（使用最下方有效行）
            int bottom_mid = -1;
            for(int row = line_tracking_height-1; row >=0; --row) {
                if(mid_line[row] != -1) {
                    bottom_mid = mid_line[row];
                    break;
                }
            }
        
        // // 有效中线检测时执行控制
        // if(bottom_mid != -1) {
        //     controlCar(bottom_mid);
        // } else {
        //     // 丢失路径处理（如停车或搜索）
        //     servo.setDutyCycle(BASE_DUTY);
        //     motorController[0]->setSpeed(0);
        //     motorController[1]->setSpeed(0);
        // }
    }
      
    
    return 0;
}